use rp235x_hal::clocks;
use rp235x_hal::dma::{Channel, ChannelIndex};
use rp235x_hal::pio::{PIOExt, StateMachine, StateMachineIndex};

const PIO_I2S_PIO_INSTRUCTIONS_PER_BIT: u32 = 2;
const PIO_I2S_NUM_CHANNELS: u32 = 2;

pub struct PioI2SConfig {
    pub data_pin: u8,
    pub bclk_pin: u8,
    pub bit_depth: u32,
    pub sample_rate: i32,
    pub block_size: usize,
}

pub struct PioI2S {
    config: PioI2SConfig,
    stereo_block_size: usize,
    output_double_buffer: &'static mut [i32],
    buffer_pointers: [*mut i32; 2],
    buffer_pointer_idx: i32,
}

impl PioI2S {
    pub fn new(config: PioI2SConfig, output_double_buffer: &'static mut [i32]) -> Self {
        let stereo_block_size = config.block_size * PIO_I2S_NUM_CHANNELS as usize;
        let buffer_pointers = [
            output_double_buffer.as_mut_ptr(),
            unsafe { output_double_buffer.as_mut_ptr().add(stereo_block_size) },
        ];
        
        Self {
            config,
            stereo_block_size,
            output_double_buffer,
            buffer_pointers,
            buffer_pointer_idx: 0,
        }
    }

    pub fn calculate_clock_division(&self) -> f32 {
        let clock_speed = clocks::clk_sys_freq() as f32;
        let bclk_speed = self.config.sample_rate as f32 * PIO_I2S_NUM_CHANNELS as f32 * self.config.bit_depth as f32;
        let pio_speed = bclk_speed * PIO_I2S_PIO_INSTRUCTIONS_PER_BIT as f32;
        clock_speed / pio_speed
    }

    pub fn verify_pio_clock_division(frequency_ratio: f32) {
        if frequency_ratio < 1.0 {
            panic!("PIO clock ratio {} is faster than the system clock", frequency_ratio);
        }
        let fractional = frequency_ratio.fract();
        let scaled = fractional * 16.0;
        if scaled.fract() > 0.0 {
            panic!("PIO clock ratio {} cannot be represented accurately", frequency_ratio);
        }
    }

    pub fn init_pio_program(&mut self) {
        let clock_division = self.calculate_clock_division();
        Self::verify_pio_clock_division(clock_division);
    }

    pub fn init_dma(&mut self) {
        // DMA initialization
    }

    pub fn start(&mut self) {
        // Start I2S output
    }

    pub fn next_output_buffer(&mut self) -> *mut i32 {
        let buffer_to_fill = self.buffer_pointers[self.buffer_pointer_idx as usize];
        self.buffer_pointer_idx = 1 - self.buffer_pointer_idx;
        buffer_to_fill
    }

    pub fn end_dma_interrupt_handler(&mut self) {
        // Clear DMA interrupt
    }

    pub fn float_to_int32(sample: f32) -> i32 {
        (sample * 2147483647.0) as i32
    }

    pub fn write_samples(left: i32, right: i32, interleaved_output: &mut [i32], i: usize) {
        interleaved_output[i * 2] = left;
        interleaved_output[i * 2 + 1] = right;
    }

    pub fn write_stereo(&self, left: &[f32], right: &[f32], interleaved_output: &mut [i32]) {
        for i in 0..left.len() {
            let left_converted = Self::float_to_int32(left[i]);
            let right_converted = Self::float_to_int32(right[i]);
            Self::write_samples(left_converted, right_converted, interleaved_output, i);
        }
    }

    pub fn write_mono(&self, mono: &[f32], interleaved_output: &mut [i32]) {
        for (i, &sample) in mono.iter().enumerate() {
            let converted = Self::float_to_int32(sample);
            Self::write_samples(converted, converted, interleaved_output, i);
        }
    }
}